#include <string.h>
#include <math.h>

#include "oslib.h"
#include "ps2_handle_spi.h"
#include "ctrl_protocol.h"

#define CAN_JOYSTICK_ID 324
#define CAN_BUTTON_ID   325
#define CAN_BUTTON_NEW_ID 326

uint8_t debug_mode = 0;

/**
 * @brief 组装CAN总线按键报文
 * 
 * @param msg   按键报文目标位置
 * @param group 按键组号
 * @param btn   按键号
 * @param press 按键状态
 * @return CAN_Message* 按键报文目标位置(同参数msg)
 */
static CAN_Message *CAN_Button_Message(CAN_Message *msg, uint8_t group, uint8_t btn, uint8_t press)
{
    memset(msg, 0, sizeof(CAN_Message));
    msg->ui8[0] = btn;
    msg->ui8[1] = group;
    msg->ui8[2] = press;
    return msg;
}

static CAN_Message *CAN_Button_Message_New(CAN_Message *msg, uint16_t btn, uint8_t group)
{
    memset(msg, 0, sizeof(CAN_Message));
    msg->ui16[0] = btn;
    msg->ui16[1] = group;
    return msg;
}

/**
 * @brief 组装CAN总线摇杆报文
 * 
 * @param msg   摇杆报文目标位置
 * @param ry    右摇杆Y轴AD值
 * @param rx    右摇杆X轴AD值
 * @param ly    左摇杆Y轴AD值
 * @param lx    左摇杆X轴AD值
 * @return CAN_Message* 摇杆报文目标位置(同参数msg)
 */
static CAN_Message *CAN_Joystick_Message(CAN_Message *msg, int8_t ry, int8_t rx, int8_t ly, int8_t lx)
{
    memset(msg, 0, sizeof(CAN_Message));
    msg->i16[0] = (int16_t)ry;
    msg->i16[1] = (int16_t)rx;
    msg->i16[2] = (int16_t)ly;
    msg->i16[3] = (int16_t)lx;
    return msg;
}

/**
 * @brief 组装CAN总线摇杆报文(左摇杆为油门型)
 * 
 * @param msg   摇杆报文目标位置
 * @param ry    右摇杆Y轴AD值
 * @param rx    右摇杆X轴AD值
 * @param ly    左摇杆Y轴AD值
 * @param lx    左摇杆X轴AD值 
 * @return CAN_Message* 摇杆报文目标位置(同参数msg)
 */
static CAN_Message *CAN_Joystick_Plus_Message(CAN_Message *msg, int8_t ry, int8_t rx, uint8_t ly, int8_t lx)
{
    memset(msg, 0, sizeof(CAN_Message));
    msg->i16[0] = (int16_t)ry;
    msg->i16[1] = (int16_t)rx;
    msg->ui16[2] = (uint16_t)ly;
    msg->i16[3] = (int16_t)lx;
    return msg;
}

static CAN_Message can_control_msg;

#include "oslib_plus_scope.h"
void Uart_Scope_Joystick(float ry, float rx, float ly, float lx)
{
    static float data_array[4];
    data_array[0] = ry;
    data_array[1] = rx;
    data_array[2] = ly;
    data_array[3] = lx;
    OSLIB_Plus_Uart_Scope(data_array, 4);
}

void Uart_Print_Joystick(float ry, float rx, float ly, float lx)
{
    uprintf(" %6.1f %6.1f %6.1f %6.1f\r\n", ry, rx, ly, lx);
}

static const uint8_t CtrlProt_Mapping[] = {
    0, // X
    1, // Y
    2, // A
    3, // B
    6, // U
    7, // R
    8, // L
    9, // D
    4, // JL
    5, // JR
};

static const uint8_t PS2_Button_Mapping[] = {
    -1, // PSB_SELECT
    4,  // PSB_L3
    5,  // PSB_R3
    -1, // PSB_START
    6,  // PSB_PAD_UP
    7,  // PSB_PAD_RIGHT
    9,  // PSB_PAD_DOWN
    8,  // PSB_PAD_LEFT
    // ...
    0, // PSB_TRIANGLE
    2, // PSB_CIRCLE
    3, // PSB_CROSS
    1, // PSB_SQUARE
};

static const uint8_t PS2_Group_Mapping[] = {
    4, // PSB_L2
    2, // PSB_R2
    3, // PSB_L1
    1, // PSB_R1
};

extern osMessageQueueId_t CtrlRecvQueueHandle;
extern osMessageQueueId_t CtrlSendQueueHandle;
extern osTimerId_t BlinkTimerHandle;
enum query_state_t
{
    QUERY_STATE_INIT,
    QUERY_STATE_RUN,
} query_state = QUERY_STATE_INIT;

enum query_mode_t
{
    QUERY_MODE_CTRL,
    QUERY_MODE_PS2,
    QUERY_MODE_UNKNOWN = -1,
} query_mode = QUERY_MODE_UNKNOWN;

#define ps2_button_remap(btn) ((((btn)&0xF000) >> 4) | ((btn)&0x00F6) | (((btn)&0x0F00) << 4))
#define ps2_button_no_group(btn) ((btn)&0x0FFF)
#define ps2_button_get_group(btn) (((btn)&0xF000) >> 12)

#define ctrl_button_remap(btn) (((btn)&0x000F) | (((btn)&0x00F0) << 2) | (((btn)&0x0300) >> 4))

static ctrl_prot_t ack = {
    .op = CTRLPROT_OP_HOST_ACK,
    .len = sizeof(ctrl_prot_data_ack_t),
};    

void _QueryTask(void *argument)
{
    UNUSED(argument);
    osStatus_t ret;
    uint16_t btn_value = 0;
    uint16_t btn_last_value = 0;
    uint16_t btn_changed = 0;
    uint16_t btn_group = 0;
    uint16_t btn_last_group = 0;
    uint16_t btn_pending = 0;
    ctrl_prot_t *pack;
    ctrl_joystick_t *joys;

    query_mode = (enum query_mode_t)(HAL_GPIO_ReadPin(Switch_GPIO_Port, Switch_Pin));

    for (;;)
    {
        switch (query_state)
        {
        case QUERY_STATE_INIT:
            if (query_mode == QUERY_MODE_PS2)
            {
                PS2_Init();
                while (PS2_Light() != 0)
                {
                    HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
                    osDelay(100);
                    PS2_Init();
                }
                HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET);
            }
            query_state = QUERY_STATE_RUN;
            break;
        case QUERY_STATE_RUN:
            if (query_mode == QUERY_MODE_PS2)
            {
                osDelay(20);
                PS2_ClearData();
                PS2_ReadData(scan, 9);
                btn_value = ~PS2_KeyData();
                if (PS2_Light() != 0)
                {
                    query_state = QUERY_STATE_INIT;
                    break;
                }
                btn_value = ps2_button_remap(btn_value);
                btn_changed = btn_value ^ btn_last_value;
                if (btn_changed != 0)
                {
                    btn_group = 0;
                    for (int i = 0; i < 4; i++)
                    {
                        if ((1UL << i) & ps2_button_get_group(btn_value))
                        {
                            btn_group = PS2_Group_Mapping[i];
                            break;
                        }
                    }

                    if (btn_group != btn_last_group) // 分组发生改变
                    {
                        // 旧按键直接释放
                        for (int i = 0; i < 12; i++)
                        {
                            if (((1UL << i) & ps2_button_no_group(btn_last_value)) /* && (PS2_Button_Mapping[i] != -1)*/)
                            {
                                if ((1UL << i) & btn_pending)
                                {
                                    btn_pending &= ~(1UL << i); // 释放抵消
                                    continue;
                                }
                                can_send_msg(CAN_BUTTON_ID,
                                             CAN_Button_Message(&can_control_msg, btn_last_group, PS2_Button_Mapping[i], 0));
                            }
                        }
                        // 新按键(不允许)直接按下
                        for (int i = 0; i < 12; i++)
                        {
                            if (((1UL << i) & ps2_button_no_group(btn_value)) /* && (PS2_Button_Mapping[i] != -1)*/)
                            {
                                // can_send_msg(CAN_BUTTON_ID,
                                //             CAN_Button_Message(&can_control_msg, btn_group, PS2_Button_Mapping[i], 1));
                                btn_pending |= (1UL << i); // 下次释放会被抵消
                            }
                        }
                    }
                    else // 分组不变
                    {
                        // 检查发生变化的位, 依次进行按下/释放
                        for (int i = 0; i < 12; i++)
                        {
                            if (((1UL << i) & ps2_button_no_group(btn_changed)) /* && (PS2_Button_Mapping[i] != -1)*/)
                            {
                                uint8_t press = ((1UL << i) & ps2_button_no_group(btn_value)) ? 1 : 0;
                                if (press == 0 && ((1UL << i) & btn_pending))
                                {
                                    btn_pending &= ~(1UL << i); // 释放抵消
                                    continue;
                                }
                                can_send_msg(CAN_BUTTON_ID,
                                             CAN_Button_Message(&can_control_msg, btn_group, PS2_Button_Mapping[i], press));
                            }
                        }
                    }

                    btn_last_group = btn_group;
                    btn_last_value = btn_value;
                }

                can_send_msg(CAN_JOYSTICK_ID, CAN_Joystick_Message(&can_control_msg,
                                                                   127 - PS2_AnalogData(PSS_RY), PS2_AnalogData(PSS_RX) - 128,
                                                                   127 - PS2_AnalogData(PSS_LY), PS2_AnalogData(PSS_LX) - 128));

                if (debug_mode == 1)
                {
                    Uart_Scope_Joystick((float)127 - PS2_AnalogData(PSS_RY), (float)PS2_AnalogData(PSS_RX) - 128,
                                        (float)127 - PS2_AnalogData(PSS_LY), (float)PS2_AnalogData(PSS_LX) - 128);
                }
                else if (debug_mode == 2)
                {
                    Uart_Print_Joystick((float)127 - PS2_AnalogData(PSS_RY), (float)PS2_AnalogData(PSS_RX) - 128,
                                        (float)127 - PS2_AnalogData(PSS_LY), (float)PS2_AnalogData(PSS_LX) - 128);
                }
            }
            else if (query_mode == QUERY_MODE_CTRL)
            {
                ret = osMessageQueueGet(CtrlRecvQueueHandle, &pack, NULL, 100);
                if (ret == osErrorTimeout) {
                    HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
                    can_send_msg(CAN_JOYSTICK_ID, CAN_Joystick_Message(&can_control_msg, 0, 0, 0, 0));
                }
                if (ret != osOK)
                    break;

                HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET);
                if (pack->op != CTRLPROT_OP_DEVICE_CONTROL && pack->op != CTRLPROT_OP_DEVICE_CONTROLP)
                    break;

                /* 处理按键数据 */
                btn_value = (((ctrl_prot_data_control_t *)(pack->data))->button);
                btn_changed = btn_value ^ btn_last_value;
                if (btn_changed != 0)
                {
                    btn_group = (ctrl_button_get_group(btn_value));
                    if (btn_group != btn_last_group) // 分组发生改变
                    {
                        // 旧按键直接释放
                        for (int i = 0; i < 10; i++)
                        {
                            if ((1UL << i) & ctrl_button_no_group(btn_last_value))
                            {
                                if ((1UL << i) & btn_pending)
                                {
                                    btn_pending &= ~(1UL << i); // 释放抵消
                                    continue;
                                }
                                can_send_msg(CAN_BUTTON_ID,
                                             CAN_Button_Message(&can_control_msg, btn_last_group, CtrlProt_Mapping[i], 0));
                            }
                        }
                        // 新按键(不允许)直接按下
                        for (int i = 0; i < 10; i++)
                        {
                            if ((1UL << i) & ctrl_button_no_group(btn_value))
                            {
                                // can_send_msg(CAN_BUTTON_ID,
                                //             CAN_Button_Message(&can_control_msg, ctrl_button_get_group(btn_value), CtrlProt_Mapping[i], 1));
                                btn_pending |= (1UL << i); // 下次释放会被抵消
                            }
                        }
                    }
                    else // 分组未发生改变
                    {
                        // 检查发生变化的位, 依次进行按下/释放
                        for (int i = 0; i < 10; i++)
                        {
                            if ((1UL << i) & ctrl_button_no_group(btn_changed))
                            {
                                uint8_t press = ((1UL << i) & ctrl_button_no_group(btn_value)) ? 1 : 0;
                                if (press == 0 && ((1UL << i) & btn_pending))
                                {
                                    btn_pending &= ~(1UL << i); // 释放抵消
                                    continue;
                                }
                                can_send_msg(CAN_BUTTON_ID,
                                             CAN_Button_Message(&can_control_msg, btn_group, CtrlProt_Mapping[i], press));
                            }
                        }
                    }

                    btn_last_group = btn_group;
                    btn_last_value = btn_value;
                }
                can_send_msg(CAN_BUTTON_NEW_ID, CAN_Button_Message_New(&can_control_msg, 
                                                                        ctrl_button_remap(ctrl_button_no_group(btn_value)), 
                                                                        ctrl_button_get_group(btn_value)));


                /* 处理摇杆数据 */
                joys = &(((ctrl_prot_data_control_t *)(pack->data))->joystick);

                if (pack->op == CTRLPROT_OP_DEVICE_CONTROL) 
                {
                    can_send_msg(CAN_JOYSTICK_ID, CAN_Joystick_Message(&can_control_msg,
                                                                        joys->right_y, joys->right_x,
                                                                        joys->left_y, joys->left_x));
                    if (debug_mode == 1)
                    {
                        Uart_Scope_Joystick(joys->right_y, joys->right_x, joys->left_y, joys->left_x);
                    }
                    else if (debug_mode == 2)
                    {
                        Uart_Print_Joystick(joys->right_y, joys->right_x, joys->left_y, joys->left_x);
                    }
                }
                else if (pack->op == CTRLPROT_OP_DEVICE_CONTROLP)
                {
                    can_send_msg(CAN_JOYSTICK_ID, CAN_Joystick_Plus_Message(&can_control_msg,
                                                                            joys->right_y, joys->right_x,
                                                                            joys->left_y, joys->left_x));
                    if (debug_mode == 1)
                    {
                        Uart_Scope_Joystick(joys->right_y, joys->right_x, (uint8_t)joys->left_y, joys->left_x);
                    }
                    else if (debug_mode == 2)
                    {
                        Uart_Print_Joystick(joys->right_y, joys->right_x, (uint8_t)joys->left_y, joys->left_x);
                    }
                }

                /* 回送ack */
                ((ctrl_prot_data_ack_t *)(ack.data))->tx_ticks = pack->ticks;
                osMessageQueuePut(CtrlSendQueueHandle, &ack, NULL, 0);
            }
            break;
        default:
            break;
        }
    }
}
